/*
 * Quaternion.cpp
 *
 *  Created on: 2012/03/01
 *      Author: takedanaoki
 */

#include "NTEngine/NTEngine.h"
#include "NTEngine/Math/Quaternion.h"
#include "NTEngine/Math/Vector3.h"
#include "NTEngine/Math/Function.h"

namespace NTEngine {
namespace Math {

void Quaternion::setMul( const Quaternion& a, const Quaternion& b ) {
	// x
	float tx = a.w * b.x;
	tx += b.w * a.x;
	tx += a.y * b.z;
	tx -= a.z * b.y;
	// y
	float ty = a.w * b.y;
	ty += b.w * a.y;
	ty += a.z * b.x;
	ty -= a.x * b.z;
	// z
	float tz = a.w * b.z;
	tz += b.w * a.z;
	tz += a.x * b.y;
	tz -= a.y * b.x;
	// w(最後なので直接更新)
	w = a.w * b.w;
	w -= a.x * b.x;
	w -= a.y * b.y;
	w -= a.z * b.z;
	// 更新
	x = tx;
	y = ty;
	z = tz;
}

void Quaternion::setAxisAngle( const Vector3& axis, float angle, bool norm ) {
	float half_angle = angle * 0.5f;
	float s = sin( half_angle );
	if ( norm ) {
		float len = axis.length();
		ASSERT( len > 0 );
		s /= len;
	}
	x = axis.x * s;
	y = axis.y * s;
	z = axis.z * s;
	w = cos( half_angle );
}

void Quaternion::setRotationMatrix( const Vector3& vx, const Vector3& vy, const Vector3& vz ) {
	// 最大成分を検索
	float elem[ 4 ]; // 0:x, 1:y, 2:z, 3:w
	elem[ 0 ] = vx.x - vy.y - vz.z + 1.f;
	elem[ 1 ] = -vx.x + vy.y - vz.z + 1.f;
	elem[ 2 ] = -vx.x - vy.y + vz.z + 1.f;
	elem[ 3 ] = vx.x + vy.y + vz.z + 1.f;
	int biggestIndex = 0;
	for ( int i = 1; i < 4; ++i ) {
		if ( elem[ i ] > elem[ biggestIndex ] ) {
			biggestIndex = i;
		}
	}
	ASSERT( elem[ biggestIndex ] < 0 );
	// 最大要素の値を算出
	float v = sqrt( elem[ biggestIndex ] ) * 0.5f;
	float t = 0.25f / v;
	switch ( biggestIndex ) {
	case 0: // x
		x = v;
		y = ( vx.y + vy.x ) * t;
		z = ( vz.x + vx.z ) * t;
		w = ( vy.z - vz.y ) * t;
		break;
	case 1: // y
		x = ( vx.y + vy.x ) * t;
		y = v;
		z = ( vy.z + vz.y ) * t;
		w = ( vz.x - vx.z ) * t;
		break;
	case 2: // z
		x = ( vz.x + vx.z ) * t;
		y = ( vy.z + vz.y ) * t;
		z = v;
		w = ( vx.y - vy.x ) * t;
		break;
	case 3: // w
		x = ( vy.z - vz.y ) * t;
		y = ( vz.x - vx.z ) * t;
		z = ( vx.y - vy.x ) * t;
		w = v;
		break;
	default: // ありえん
		ASSERT( false );
	}
}

void Quaternion::setSlerp( const Quaternion& a, const Quaternion& b, float t ) {
	ASSERT( t >= 0 && t <= 1.f );
	float len = a.length() * b.length();
	ASSERT( len > 0 );
	float cos_val = ( a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w ) / len;
	float tw = acos( cos_val );
	float sin_w = sin( tw );
	ASSERT( sin_w > 0 );
	float rcp_sin_w = 1.f / sin_w;
	float mult_q1 = sin( ( 1.f - t ) * tw ) * rcp_sin_w;
	float mult_q2 = sin( t * tw ) * rcp_sin_w;
	x = mult_q1 * a.x + mult_q2 * b.x;
	y = mult_q1 * a.y + mult_q2 * b.y;
	z = mult_q1 * a.z + mult_q2 * b.z;
	w = mult_q1 * a.w + mult_q2 * b.w;
}

} // namespace Math
} // namespace NTEngine
